/**
  ******************************************************************************
  * @file    system_stm32mp13xx_A7.c
  * @author  MCD Application Team
  * @brief   CMSIS Cortex Device Peripheral Access Layer System Source File.
  *
  *   This file provides two functions and one global variable to be called from
  *   user application:
  *      - SystemInit(): This function is called at startup just after reset and
  *                      before branch to main program. This call is made inside
  *                      the "startup_stm32mp13xx.s" file.
  *
  *      - SystemCoreClock variable: Contains the core clock frequency, it can
  *                                  be used by the user application to setup
  *                                  the SysTick timer or configure other
  *                                  parameters.

  *      - SystemCoreClockUpdate(): Updates the variable SystemCoreClock and must
  *                                 be called whenever the core clock is changed
  *                                 during program execution.
  *
  *
  ******************************************************************************
  * @attention
  *
  * Copyright (c) 2020-2021 STMicroelectronics.
  * All rights reserved.
  *
  * This software is licensed under terms that can be found in the LICENSE file
  * in the root directory of this software component.
  * If no LICENSE file comes with this software, it is provided AS-IS.
  *
  ******************************************************************************
  */

/** @addtogroup CMSIS
  * @{
  */

/** @addtogroup stm32mp13xx_system
  * @{
  */

/** @addtogroup STM32MP13xx_System_Private_Includes
  * @{
  */

#include "stm32mp13xx.h"
#include "irq_ctrl.h"
#include "math.h"

/**
  * @}
  */

/** @addtogroup STM32MP13xx_System_Private_TypesDefinitions
  * @{
  */


/**
  * @}
  */

/** @addtogroup STM32MP13xx_System_Private_Defines
  * @{
  */
#if defined ( __ICCARM__ )
extern uint32_t ZI_DATA;
extern uint32_t __BSS_END__;
extern  IRQHandler_t IRQ_Vector_Table[MAX_IRQ_n];
extern void ZeroBss( void );
#endif


/************************* Miscellaneous Configuration ************************/
/*!< Uncomment the following line if you need to use external SRAM mounted
     on EVAL board as data memory  */


/* This defines are associated to GICv2 IP from ARM */
#define GIC_HIGHEST_INTERRUPT_VALUE 1020U
#define GIC_HIGHEST_SGI_PPI_VALUE     31U
#define GIC_HIGHEST_SGI_VALUE         15U

/*
 * ID 1023. This value is returned to a processor, in response to an interrupt acknowledge,
 * if there is no pending interrupt with sufficient priority for
 * it to be signaled to the processor.
 */
#define GIC_ACKNOWLEDGE_RESPONSE 1023U

/*  GICC_IAR bit assignments
 * [31:13] - Reserved.
 * [12:10] CPUID For SGIs in a multiprocessor implementation, this field identifies the processor that
 * requested the interrupt. It returns the number of the CPU interface that made the
 * request, for example a value of 3 means the request was generated by a write to the
 * GICD_SGIR on CPU interface 3.
 * For all other interrupts this field is RAZ.
 * [9:0] Interrupt ID The interrupt ID
 */
#define GICC_IAR_VALID_BITMASK    0x3FF
#define GICC_IAR_CPU_ID_BITMASK  0x1C00
#define GICC_IAR_CPU_ID_SHIFT        10

#define __GIC_GET_CPU_ID(__IAR)  (((__IAR) & GICC_IAR_CPU_ID_BITMASK) >> GICC_IAR_CPU_ID_SHIFT)


/******************************************************************************/

/**
  * @}
  */

/** @addtogroup STM32MP13xx_System_Private_Macros
  * @{
  */

/**
  * @}
  */

/** @addtogroup STM32MP13xx_System_Private_Variables
  * @{
  */
  /* This variable is updated in three ways:
      1) by calling CMSIS function SystemCoreClockUpdate()
      2) each time HAL_RCC_ClockConfig() is called to configure the system clock
         frequency
         Note: If you use this function to configure the system clock;
               then there is no need to call the first functions listed above,
               since SystemCoreClock variable is updated automatically.
  */
  uint32_t SystemCoreClock = HSI_VALUE;
/**
  * @}
  */

/** @addtogroup STM32MP13xx_System_Private_FunctionPrototypes
  * @{
  */

/**
  * @brief  Calculates PLL1 P frequency based on internal RCC configuration
  *         registers.
  * @param  None
  * @retval Calculated frequency on [Hz]
  */
static uint32_t GetPLL1PClockFreq(void)
{
  uint32_t   pllsource, pll1m, pll1fracen, pll1p_freq;
  float_t fracn1, pll1vco;

  pllsource = (RCC->RCK12SELR & RCC_RCK12SELR_PLL12SRC);
  pll1m = ((RCC->PLL1CFGR1 & RCC_PLL1CFGR1_DIVM1)>> RCC_PLL1CFGR1_DIVM1_Pos) + 1U;
  pll1fracen = (RCC->PLL1FRACR & RCC_PLL1FRACR_FRACLE) >> 16U;
  fracn1 = ((float_t)(uint32_t)(pll1fracen * ((RCC->PLL1FRACR & RCC_PLL1FRACR_FRACV) >> 3)));
  pll1vco = ((float_t)(uint32_t)(((RCC->PLL1CFGR1 & RCC_PLL1CFGR1_DIVN) + 1U)) + (fracn1/(float_t)0x1FFF));

  if (pll1m != 0U)
  {
    switch (pllsource)
    {
      case 0x00:  /* HSI used as PLL clock source */
       pll1vco *= ((float_t)(uint32_t)(HSI_VALUE >> (RCC->HSICFGR & RCC_HSICFGR_HSIDIV )) / (float_t)pll1m);
       break;

      case 0x01:  /* HSE used as PLL clock source */
        pll1vco *= ((float_t)(uint32_t)HSE_VALUE / (float_t)pll1m);
        break;

      case 0x02:  /* No clock source for PLL */
        pll1vco = (float_t)0;
        break;

      default:
        break;
    }
    pll1p_freq = (((uint32_t)(float_t)pll1vco)/(uint32_t)((RCC->PLL1CFGR2 & RCC_PLL1CFGR2_DIVP) + 1U));
  }
  else
  {
    pll1p_freq = 0U;
  }
  return pll1p_freq;
}

#if defined (DATA_IN_ExtSRAM)
  static void SystemInit_ExtMemCtl(void);
#endif /* DATA_IN_ExtSRAM */

/** @addtogroup STM32MP13xx_System_Private_Functions
  * @{
  */

/**
  * @brief  Generic "IRQ" Error Handler
  * @param  None
  * @retval None
  */
static void SystemInit_IRQ_ErrorHandler(void)
{
#if !defined(USE_SECOND_A7_CORE)
  /* Behavior in case of Exception or IRQ handling Errors */
  while(1U);
#endif
}

__weak void SystemInit_Interrupts_SoftIRQn_Handler(uint32_t Software_Interrupt_Id, uint8_t cpu_id_request)
{
  /* Prevent unused argument(s) compilation warning */
  UNUSED((void)Software_Interrupt_Id);
  UNUSED((void)cpu_id_request);
}

void SecurePhysicalTimer_IRQHandler(void)
{
  IRQ_ClearPending((IRQn_ID_t)SecurePhysicalTimer_IRQn);

#if 0
/* Simple version
   same as ARM CMSIS-Core-A for RTX OS

   Doesn't compensate for IRQ processing time (hardware and software)

   Measured error with non optimized compilation (-O0) is
     193 CLK/tick = 193/48000 = 0.40 % = 4ms/s
   Measured error with optimized compilation (-O2) is
     110 CLK/tick = 110/48000 = 0.23 % = 2.3ms/s
*/

  PL1_SetLoadValue(HSI_VALUE/1000);
#endif /* 0 */

#if 0
/* Assembly version
   Best result whatever optimization used for compilation

   Compensates for IRQ processing time (hardware and software) by adding current counter value
     which decrements below 0 after IRQ trigger and thus contains -(time since IRQ triggered)
   As if is basic assembly, it is not semsitive to optimization setting

   Measured error with non optimized compilation (-O0) is
     6 CLK/tick = 6/48000 = 0.01 % = 0.1ms/s
   Measured error with optimized compilation (-O2) is
     6 CLK/tick = 6/48000 = 0.01 % = 0.1ms/s
*/

  __ASM volatile("MRC p15, 0, r3, c14, c2, 0\n"
                 "ADD r3, r3, #47872\n"
                 "ADD r3, r3, #128\n"
                 "MCR p15, 0, r3, c14, c2, 0":::"r3");
#endif /* 0 */

/* C version with IRQ processing time compensation.
   Same method as assembly version but written in C using CMSIS-Core-A API.

   Same result as Assembly version when optimized because functions are inlined and generated code is same as assembly version
   Result is not good when not optimized because "inline" functions are not inlined by GCC when optimization is off.
     So time for functions calls between Counter read and counter write are not compensated

   Compensates for IRQ processing time (hardware and software) by adding current counter value
     which decrements below 0 after IRQ trigger and thus contains -(time since IRQ triggered)

   Measured error with non optimized compilation (-O0) is
     34 CLK/tick = 34/48000 = 0.07 % = 0.7ms/s
   Measured error with optimized compilation (-O2) is
     6 CLK/tick = 6/48000 = 0.01 % = 0.1ms/s
*/

  PL1_SetLoadValue((HSI_VALUE/1000U) + PL1_GetCurrentValue());

  HAL_IncTick();
}

/**
  * @brief  Generic IRQ Handler (Software IRQs, PPIs & IRQs)
  * @param  None
  * @retval None
  */
#if defined ( __GNUC__ )
void __attribute__ ((interrupt ("IRQ")))IRQ_Handler(void) {
#elif defined ( __ICCARM__ )
__irq __arm void IRQ_Handler(void) {
#endif
  uint32_t ItId;
  IRQHandler_t handler;

  while (1U)
  {
    /* Get highest pending Interrupt Id from GIC driver*/
    ItId = (uint32_t)IRQ_GetActiveIRQ();

    if (ItId <= GIC_HIGHEST_INTERRUPT_VALUE) /* Highest value of GIC Valid Interrupt */
    {
      /* Check validity of IRQ */
      if (ItId >= (uint32_t)MAX_IRQ_n)
      {
        SystemInit_IRQ_ErrorHandler();
      }
      else
      {
        /* Find appropriate IRQ Handler (Require registration before!) */
        handler = IRQ_GetHandler((IRQn_ID_t)ItId);

        if (handler!=NULL)
        {
          /* Call IRQ Handler */
          handler();
        }
        else
        {
          /* Un register Handler , error ! */
          SystemInit_IRQ_ErrorHandler();
        }
      }

      /* End Acknowledge interrupt */
      IRQ_EndOfInterrupt((IRQn_ID_t)ItId);
    }
    else
    {
      /* Normal case: whenever there is no more pending IRQ , IAR returns ACKNOWLEDGE special IRQ value */
      if (ItId == GIC_ACKNOWLEDGE_RESPONSE)
      {
        break;
      }
      /* Spurious IRQ Value (1022)  ... */
      else
      {
        SystemInit_IRQ_ErrorHandler();
      }
    }
  }
}

/**
  * @brief  Ensure all bss part of code is initialized with zeros
  * @param  None
  * @retval None
  */
#if defined ( __GNUC__ )
void __attribute__((noinline)) ZeroBss(void)
{
  __asm volatile(
    "PUSH {R4-R11}          \n"
    "LDR r2, =ZI_DATA       \n"
    "b LoopFillZerobss      \n"
    /* Zero fill the bss segment. */
    "FillZerobss:           \n"
    "MOVS r3, #0            \n"
    "STR  r3, [r2]          \n"
    "adds r2, r2, #4        \n"

    "LoopFillZerobss:       \n"
    "LDR r3, = __BSS_END__  \n"
    "CMP r2, r3             \n"
    "BCC FillZerobss        \n"

    "DSB                    \n"
    "POP    {R4-R11}        "
  );
}
#endif
  /**
  * @brief  Setup the microcontroller system
  *         Initialize the FPU setting, vector table location and External memory
  *         configuration.
  * @param  None
  * @retval None
  */
void SystemInit (void)
{

#if !defined ( __ICCARM__ )
  /* Fill BSS Section with '0' */
  ZeroBss();
#endif

  /* Invalidate entire Unified TLB */
  __set_TLBIALL(0);

  /* Disable all interrupts and events */
  CLEAR_REG(EXTI_C1->IMR1);
  CLEAR_REG(EXTI_C1->IMR2);
  CLEAR_REG(EXTI_C1->IMR3);
  CLEAR_REG(EXTI_C1->EMR1);
  CLEAR_REG(EXTI_C1->EMR2);
  CLEAR_REG(EXTI_C1->EMR3);

  /* Invalidate entire branch predictor array */
  __set_BPIALL(0);
  __DSB();
  __ISB();

  /*  Invalidate instruction cache and flush branch target cache */
  __set_ICIALLU(0);
  __DSB();
  __ISB();

  /*  Invalidate data cache */
  L1C_InvalidateDCacheAll();

#if ((__FPU_PRESENT == 1) && (__FPU_USED == 1))
  /* Enable FPU */
  __FPU_Enable();
#endif

#ifndef NO_MMU_USE
  /* Create Translation Table */
  MMU_CreateTranslationTable();

  /* Enable MMU */
  MMU_Enable();
#endif

  /* Enable Caches */
#ifndef NO_CACHE_USE
  L1C_EnableCaches();
#endif
  L1C_EnableBTAC();

#ifndef NO_CACHE_USE
#if (__L2C_PRESENT == 1)
  /* Enable L2 Cache */
  L2C_Enable();
#endif
#endif

#if (__GIC_PRESENT == 1)
  uint32_t i;

  /* Enable GIC */
  IRQ_Initialize();

  /* Set Interrupt vectors */
  for (i = 0U; i < (uint32_t)MAX_IRQ_n ; i++) {
    IRQ_SetHandler((IRQn_ID_t)i, IRQ_Vector_Table[i]);
  }
#endif
}

/**
 * @brief  Setup the microcontroller system
 *         Initialize the FPU setting, vector table location and External memory
 *         configuration for Cortex-A7 cpu1.
 * @param  None
 * @retval None
 */
void SystemInit_cpu1 (void)
{


 /* Invalidate entire Unified TLB */
 __set_TLBIALL(0);

 /* Invalidate entire branch predictor array */
 __set_BPIALL(0);
 __DSB();
 __ISB();

 /*  Invalidate instruction cache and flush branch target cache */
 __set_ICIALLU(0);
 __DSB();
 __ISB();

 /*  Invalidate data cache */
 L1C_InvalidateDCacheAll();

#if ((__FPU_PRESENT == 1) && (__FPU_USED == 1))
 /* Enable FPU */
 __FPU_Enable();
#endif

#ifndef NO_MMU_USE
 /* Create Translation Table */
 MMU_CreateTranslationTable();

 /* Enable MMU */
 MMU_Enable();
#endif

 /* Enable Caches */
#ifndef NO_CACHE_USE
 L1C_EnableCaches();
#endif
 L1C_EnableBTAC();

#ifndef NO_CACHE_USE
#if (__L2C_PRESENT == 1)
 /* Enable L2 Cache */
 L2C_Enable();
#endif
#endif

}

/**
   * @brief Update SystemCoreClock variable according to Clock Register Values.
  *         The SystemCoreClock variable contains the core clock frequency (Hz),
  *         it can be used by the user application to setup the SysTick timer or
  *         configure other parameters.
  *
  * @note   Each time the core clock changes, this function must be called to
  *         update SystemCoreClock variable value. Otherwise, any configuration
  *         based on this variable will be incorrect.
  *
  * @note   - The system frequency computed by this function is not the real
  *           frequency in the chip. It is calculated based on the predefined
  *           constant and the selected clock source:
  *
  *           - If SYSCLK source is HSI, SystemCoreClock will contain the
  *             HSI_VALUE(*)
  *
  *           - If SYSCLK source is HSE, SystemCoreClock will contain the
  *             HSE_VALUE(**)
  *
  *           - If SYSCLK source is PLL1_P, SystemCoreClock will contain the
  *             HSI_VALUE(*) or the HSE_VALUE(*) multiplied/divided by the PLL3
  *             factors.
  *
  *           - If SYSCLK source is MPUDIV, SystemCoreClock will contain the
  *             HSI_VALUE(*) or the HSE_VALUE(*) multiplied/divided by the PLL3
  *             factors and then divided by the MPUDIV factor.
  *
  *         (*) HSI_VALUE is a constant defined in stm32mp13xx_hal_conf.h file
  *             (default value 64 MHz) but the real value may vary depending
  *             on the variations in voltage and temperature.
  *
  *         (**) HSE_VALUE is a constant defined in stm32mp13xx_hal_conf.h file
  *              (default value 24 MHz), user has to ensure that HSE_VALUE is
  *              same as the real frequency of the crystal used. Otherwise, this
  *              function may have wrong result.
  *
  *         - The result of this function could be not correct when using
  *           fractional value for HSE crystal.
  *
  * @param  None
  * @retval None
  */
void SystemCoreClockUpdate (void)
{
  uint32_t mpudiv;

  switch (RCC->MPCKSELR & RCC_MPCKSELR_MPUSRC)
  {
  case 0x00:  /* HSI used as PLL clock source */
    SystemCoreClock = (HSI_VALUE >> (RCC->HSICFGR & RCC_HSICFGR_HSIDIV));
    break;

  case 0x01:  /* HSE used as PLL clock source */
    SystemCoreClock = HSE_VALUE;
    break;

  case 0x02:  /* PLL1_P used as system clock source */
    SystemCoreClock = GetPLL1PClockFreq();
    break;

  case 0x03: /* MPUDIV used as system clock source */
    mpudiv = (RCC->MPCKDIVR & RCC_MPCKDIVR_MPUDIV);
    if(mpudiv == 0U)
    {
      SystemCoreClock = 0U;
    }
    else if(mpudiv <= 4U)
    {
      SystemCoreClock = (GetPLL1PClockFreq() >> mpudiv );
    }
    else
    {
      SystemCoreClock = (GetPLL1PClockFreq() >> 16U); /* divided by 16 */
    }
    break;

  default:
    break;
  }
}
/**
  * @}
  */

/**
  * @}
  */

/**
  * @}
  */

